/***************************************************************************
 *   mgpi_calculus.c - Mathematical Calulations Definitions                *
 *   Copyright (C) 2008 by cmaster.matso                                   *
 *   cmaster.matso@gmail.com                                               *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include "mgpi_math.h"

static float s_nMax = 1.0f;                                                     /** max value for clamping */
static float s_nMin = -1.0f;                                                    /** min value for clamping */
static int s_nClampMode = MGPI_ZERO_ONE;                                        /** clamping mode flag */

static int s_compare(const void *a, const void *b) {
    float v0 = *((float *)a);
    float v1 = *((float *)b);

    if (v0 < v1) return -1;
    else if (v0 == v1) return 0;
    else return 1;
}

/**
 * Sets the given value as min value in clamping.
 */
float MGPI_Min(float value) {
    return (s_nMin = value);
}

/**
 * Sets the given value as max value in clamping.
 */
float MGPI_Max(float value) {
    return (s_nMax = value);
}

/**
 * Sets the clamp mode to the given.
 */
void MGPI_ClampMode(int mode) {
    s_nClampMode = mode;
    return;
}

/**
 * Clamps the given value according to the set max and min values
 * and to the selected clamping range (-1 to 1 or 0 to 1).
 */
float MGPI_Clamp(float x) {
    if (s_nClampMode) return ((x - s_nMin) / (s_nMax - s_nMin));
    else return (2 * ((x - s_nMin) / (s_nMax - s_nMin)) - 1);
}

/**
 * Unclamps the given value according to the set max and min values
 * and to the selected clamping range (-1 to 1 or 0 to 1).
 */
float MGPI_Unclamp(float y) {
    if (s_nClampMode) return (y * (s_nMax - s_nMin) + s_nMin);
    else return ((y * (s_nMax - s_nMin) + s_nMax + s_nMin) / 2);
}

/**
 * Calculates length for the given vector.
 */
float MGPI_VecLength(float4 v) {
    return (sqrt((v[X] * v[X])+(v[Y] * v[Y])+(v[Z] * v[Z])));
}

/**
 * Calculates k-norm for the given vector.
 */
float MGPI_NormK(floatn v, int k, int n) {
    int i;
    float out = 0.0f;
    if (v && n > 0) {
        switch (k) {
            case 1:
                for (i = 0; i < n; ++i) out += fabs(v[i]);
                break;
            case 2:
                for (i = 0; i < n; ++i) out += pow(fabs(v[i]), 2);
                out = sqrt(out);
                break;
            default:
                MGPI_PostMessagef("%s: warrning: parameter k > 2 - using normInf", __FUN__);
                out = MGPI_NormInfinity(v, n);
                break;
        }
        return out;
    } else return -1;
}

/**
 * Calculates infinitive norm for the given vector.
 */
float MGPI_NormInfinity(floatn v, int n) {
    float ni  = -1.0f;

    if (n > 1) {
        qsort(v, n, sizeof(float), s_compare);
        ni = (v[n-1] / v[0]);/*???*/
    }

    return ni;
}

/**
 * Calculates cross product for two given vectors.
 */
float4 *MGPI_CrossProduct(float4 *product, float4 a, float4 b) {
    (*product)[0] = (a[1] * b[2] - a[2] * b[1]);
    (*product)[1] = (a[2] * b[0] - a[0] * b[2]);
    (*product)[2] = (a[0] * b[1] - a[1] * b[0]);
    (*product)[3] = 1.0f;
    return product;
}

/**
 * Calculates dot product for two given vectors.
 */
float MGPI_DotProduct(float3 a, float3 b) {
    return (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]);
}

/**
 * Normalizes the given vector by dividing it by its length.
 */
float4 *MGPI_Normalize(float4 *vect) {
    float len = MGPI_VecLength((*vect));
    (*vect)[X] /= len;
    (*vect)[Y] /= len;
    (*vect)[Z] /= len;
    (*vect)[W] /= len;
    return vect;
}

/**
 * Makes the given float3 vector uniform by dividing it by its Z component.
 */
float3 *MGPI_Uniform3(float3 *vect) {
    (*vect)[X] /= (*vect)[Z];
    (*vect)[Y] /= (*vect)[Z];
    (*vect)[Z] /= (*vect)[Z];
    return vect;
}

/**
 * Makes the given float4 vector uniform by dividing it by its W component.
 */
float4 *MGPI_Uniform4(float4 *vect) {
    (*vect)[X] /= (*vect)[W];
    (*vect)[Y] /= (*vect)[W];
    (*vect)[Z] /= (*vect)[W];
    (*vect)[W] /= (*vect)[W];
    return vect;
}

/**
 * Multiplies two 3-by-3 matrices.
 */
float3x3 *MGPI_MatrixMul3(float3x3 *product, float3x3 a, float3x3 b) {
    int i, j, k;
    float value = 0.0f;

    for (i = 0; i < 3; ++i)
        for (j = 0; j < 3; ++j) {
            value = 0.0f;
            for (k = 0; k < 3; ++k) value += a[i][k] * b[k][j];
            (*product)[i][j] = value;
        }

    return product;
}

/**
 * Multiplies two 4-by-4 matrices.
 */
float4x4 *MGPI_MatrixMul4(float4x4 *product, float4x4 a, float4x4 b) {
    int i, j, k;
    float value = 0.0f;

    for (i = 0; i < 4; ++i)
        for (j = 0; j < 4; ++j) {
            value = 0.0f;
            for (k = 0; k < 4; ++k) value += a[i][k] * b[k][j];
            (*product)[i][j] = value;
        }

    return product;
}

/**
 * Multiplies 3-by-3 matrix with float3 vector (left side).
 */
float3 *MGPI_MatrixMulVect3(float3 *product, float3x3 A, float3 v) {
    (*product)[0] = (A[0][0] * v[0] + A[0][1] * v[1] + A[0][2] * v[2]);
    (*product)[1] = (A[1][0] * v[0] + A[1][1] * v[1] + A[1][2] * v[2]);
    (*product)[2] = (A[2][0] * v[0] + A[2][1] * v[1] + A[2][2] * v[2]);
    return product;
}

/**
 * Multiplies 3-by-3 matrix with float3 vector (right side).
 */
float3 *MGPI_VectorMulMatr3(float3 *product, float3 v, float3x3 A) {
    (*product)[0] = (v[0] * A[0][0] + v[1] * A[1][0] + v[2] * A[2][0]);
    (*product)[1] = (v[0] * A[0][1] + v[1] * A[1][1] + v[2] * A[2][1]);
    (*product)[2] = (v[0] * A[0][2] + v[1] * A[1][2] + v[2] * A[2][2]);
    return product;
}

/**
 * Multiplies 4-by-4 matrix with float4 vector (left side).
 */
float4 *MGPI_MatrixMulVect4(float4 *product, float4x4 A, float4 v) {
    (*product)[0] = (A[0][0] * v[0] + A[0][1] * v[1] + A[0][2] * v[2] + A[0][3] * v[3]);
    (*product)[1] = (A[1][0] * v[0] + A[1][1] * v[1] + A[1][2] * v[2] + A[1][3] * v[3]);
    (*product)[2] = (A[2][0] * v[0] + A[2][1] * v[1] + A[2][2] * v[2] + A[2][3] * v[3]);
    (*product)[3] = (A[3][0] * v[0] + A[3][1] * v[1] + A[3][2] * v[2] + A[3][3] * v[3]);
    return product;
}

/**
 * Multiplies 4-by-4 matrix with float4 vector (right side).
 */
float4 *MGPI_VectorMulMatr4(float4 *product, float4 v, float4x4 A) {
    (*product)[0] = (v[0] * A[0][0] + v[1] * A[1][0] + v[2] * A[2][0] + v[3] * A[3][0]);
    (*product)[1] = (v[0] * A[0][1] + v[1] * A[1][1] + v[2] * A[2][1] + v[3] * A[3][1]);
    (*product)[2] = (v[0] * A[0][2] + v[1] * A[1][2] + v[2] * A[2][2] + v[3] * A[3][2]);
    (*product)[3] = (v[0] * A[0][3] + v[1] * A[1][3] + v[2] * A[2][3] + v[3] * A[3][3]);
    return product;
}

/**
 * Transposes the given 4-by-4 matrix.
 */
float4x4 *MGPI_TranspMatr4x4(float4x4 *matrix) {
    float4x4 m;
    m[0][0] = (*matrix)[0][0];
    m[0][1] = (*matrix)[1][0];
    m[0][2] = (*matrix)[2][0];
    m[0][3] = (*matrix)[3][0];
    m[1][0] = (*matrix)[0][1];
    m[1][1] = (*matrix)[1][1];
    m[1][2] = (*matrix)[2][1];
    m[1][3] = (*matrix)[3][1];
    m[2][0] = (*matrix)[0][2];
    m[2][1] = (*matrix)[1][2];
    m[2][2] = (*matrix)[2][2];
    m[2][3] = (*matrix)[3][2];
    m[3][0] = (*matrix)[0][3];
    m[3][1] = (*matrix)[1][3];
    m[3][2] = (*matrix)[2][3];
    m[3][3] = (*matrix)[3][3];

    (*matrix)[0][0] = m[0][0];
    (*matrix)[0][1] = m[0][1];
    (*matrix)[0][2] = m[0][2];
    (*matrix)[0][3] = m[0][3];
    (*matrix)[1][0] = m[1][0];
    (*matrix)[1][1] = m[1][1];
    (*matrix)[1][2] = m[1][2];
    (*matrix)[1][3] = m[1][3];
    (*matrix)[2][0] = m[2][0];
    (*matrix)[2][1] = m[2][1];
    (*matrix)[2][2] = m[2][2];
    (*matrix)[2][3] = m[2][3];
    (*matrix)[3][0] = m[3][0];
    (*matrix)[3][1] = m[3][1];
    (*matrix)[3][2] = m[3][2];
    (*matrix)[3][3] = m[3][3];
    return matrix;
}

/**
 * Converts the given 4-by-4 matrix to float array.
 */
float *MGPI_ToArray16(float4x4 *matrix) {
    float *m16 = matrix4x4();
    m16[M11] = (*matrix)[0][0];
    m16[M12] = (*matrix)[0][1];
    m16[M13] = (*matrix)[0][2];
    m16[M14] = (*matrix)[0][3];
    m16[M21] = (*matrix)[1][0];
    m16[M22] = (*matrix)[1][1];
    m16[M23] = (*matrix)[1][2];
    m16[M24] = (*matrix)[1][3];
    m16[M31] = (*matrix)[2][0];
    m16[M32] = (*matrix)[2][1];
    m16[M33] = (*matrix)[2][2];
    m16[M34] = (*matrix)[2][3];
    m16[M41] = (*matrix)[3][0];
    m16[M42] = (*matrix)[3][1];
    m16[M43] = (*matrix)[3][2];
    m16[M44] = (*matrix)[3][3];
    return m16;
}

/**
 * Converts the given float array to 4x4 matrix.
 */
float **MGPI_ToArray4x4(float *matrix) {
    return matrix4(matrix);
}

/**
 * Transposes the given 3-by-3 matrix.
 */
float3x3 *MGPI_TranspMatr3x3(float3x3 *matrix) {
    float3x3 m;
    m[0][0] = (*matrix)[0][0];
    m[0][1] = (*matrix)[1][0];
    m[0][2] = (*matrix)[2][0];
    m[1][0] = (*matrix)[0][1];
    m[1][1] = (*matrix)[1][1];
    m[1][2] = (*matrix)[2][1];
    m[2][0] = (*matrix)[0][2];
    m[2][1] = (*matrix)[1][2];
    m[2][2] = (*matrix)[2][2];

    (*matrix)[0][0] = m[0][0];
    (*matrix)[0][1] = m[0][1];
    (*matrix)[0][2] = m[0][2];
    (*matrix)[1][0] = m[1][0];
    (*matrix)[1][1] = m[1][1];
    (*matrix)[1][2] = m[1][2];
    (*matrix)[2][0] = m[2][0];
    (*matrix)[2][1] = m[2][1];
    (*matrix)[2][2] = m[2][2];
    return matrix;
}

/**
 * Converts the given 3-by-3 matrix to float array.
 */
float *MGPI_ToArray9(float3x3 *matrix) {
    float *m9 = matrix3x3();
    m9[M11] = (*matrix)[0][0];
    m9[M12] = (*matrix)[0][1];
    m9[M13] = (*matrix)[0][2];
    m9[M14] = (*matrix)[1][0];
    m9[M21] = (*matrix)[1][1];
    m9[M22] = (*matrix)[1][2];
    m9[M23] = (*matrix)[2][0];
    m9[M24] = (*matrix)[2][1];
    m9[M31] = (*matrix)[2][2];
    return m9;
}

/**
 * Converts the given float array to 3-by-3 matrix.
 */
float **MGPI_ToArray3x3(float *matrix) {
    return matrix3(matrix);
}

/**
 * Multiplies two n-by-n matrices.
 */
matrix_t *MGPI_MatrixMuln(matrix_t *product, matrix_t A, matrix_t B, int n) {
    int i, j, k;
    float value = 0.0f;

    for (i = 0; i < n; ++i)
        for (j = 0; j < n; ++j) {
            value = 0.0f;
            for (k = 0; k < n; ++k) value += A[i][k] * B[k][j];
            (*product)[i][j] = value;
        }

    return product;
}

/**
 * Multiplies m-by-n matrix with n-by-m one.
 */
matrix_t *MGPI_MatrixMulmn(matrix_t *product, matrix_t A, matrix_t B, int m, int n) {
    int i, j, k;
    float value = 0.0f;

    for (i = 0; i < m; ++i)
        for (j = 0; j < n; ++j) {
            value = 0.0f;
            for (k = 0; k < m; ++k) value += A[i][k] * B[k][j];
            (*product)[i][j] = value;
        }

    return product;
}

/**
 * Multiplies two quaternions.
 */
quat_t *MGPI_MulQuats(quat_t *p, quat_t q1, quat_t q2) {
    float4 v;

    (*p)[W] = q1[W] * q2[W] + MGPI_DotProduct(q1, q2);
    MGPI_CrossProduct(&v, q1, q2);
    (*p)[X] = q1[W] * q2[X] + q2[W] * q1[X] + v[X];
    (*p)[Y] = q1[W] * q2[Y] + q2[W] * q1[Y] + v[Y];
    (*p)[Z] = q1[W] * q2[Z] + q2[W] * q1[Z] + v[Z];

    return p;
}

/**
 * Converts the given quaternion to its reciprocal.
 */
quat_t *MGPI_ReciprocalQuat(quat_t *q1, quat_t q) {
    float qq = MGPI_QuatSqrNorm(q);

    MGPI_ConjugateQuat(q1, q);
    (*q1)[X] /= qq;
    (*q1)[Y] /= qq;
    (*q1)[Z] /= qq;
    (*q1)[W] /= qq;

    return q1;
}

/**
 * Converts the given quaternion to its conjugate.
 */
quat_t *MGPI_ConjugateQuat(quat_t *p, quat_t q) {
    (*p)[X] = -q[X];
    (*p)[Y] = -q[Y];
    (*p)[Z] = -q[Z];
    (*p)[W] = q[W];

    return p;
}

/**
 * Calculate a square norm of the given quaternion.
 */
float MGPI_QuatSqrNorm(quat_t q) {
    return sqrt((q[W] * q[W] + q[X] * q[X] + q[Y] * q[Y] + q[Z] * q[Z]));
}
